#pragma once

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <string>
#include <vector>

struct PoseInfo {
    double timestamp;
    Eigen::Vector3d rtk_position;
    Eigen::Vector3d slam_position;
    Eigen::Vector3d slam_euler;
    Eigen::Matrix<double, 6, 6> covariance;
};

class CloudTransform {
public:
    CloudTransform();
    
    bool loadPointClouds(const std::string& old_cloud_path, 
                        const std::string& new_cloud_path);
    
    bool loadPoseInfo(const std::string& pose_file_path);
    
    void transformNewCloud();
    
    double evaluateCloudDifference();
    
    pcl::PointCloud<pcl::PointXYZ>::Ptr getOldCloud() const { return old_cloud_; }
    pcl::PointCloud<pcl::PointXYZ>::Ptr getNewCloud() const { return new_cloud_; }
    pcl::PointCloud<pcl::PointXYZ>::Ptr getTransformedCloud() const { return transformed_cloud_; }

private:
    pcl::PointCloud<pcl::PointXYZ>::Ptr old_cloud_;
    pcl::PointCloud<pcl::PointXYZ>::Ptr new_cloud_;
    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud_;
    std::vector<PoseInfo> pose_info_;
    
    Eigen::Matrix4f getTransformationMatrix(const PoseInfo& pose);
};
